﻿#include "ssMatrix.h"
#include "ssAssert.h"

namespace StarSeeker
{
/////////////////////////////////float2x2//////////////////////////////////
ssFLOAT2x2X::ssFLOAT2x2X()
{
	Zero();
}

ssFLOAT2x2X::~ssFLOAT2x2X()
{
}

ssFLOAT2x2X::ssFLOAT2x2X(ssfloat p11, ssfloat p12, ssfloat p21, ssfloat p22)
{
	_11 = p11; _12 = p12;
	_21 = p21; _22 = p22;
}

ssFLOAT2x2X::ssFLOAT2x2X(const ssFLOAT2x2X &val)
{
	memcpy(m, val.m, sizeof(ssFLOAT2x2));
}

void ssFLOAT2x2X::operator=(const ssFLOAT2x2X &val)
{
	memcpy(m, val.m, sizeof(ssFLOAT2x2));
}

ssFLOAT2x2X::operator ssfloat* ()
{
	return &_11;
}

ssFLOAT2x2X::operator const ssfloat* () const
{
	return &_11;
}

void ssFLOAT2x2X::Zero()
{
	memset(m, 0, sizeof(ssFLOAT2x2));
}
//单位化
void ssFLOAT2x2X::Identity()
{
	_12 = _21 = 0.0f;
	_11 = _22 = 1.0f;
}
//转置
void ssFLOAT2x2X::Transpose()
{
	ssfloat t;
	t = _12; _12 = _21; _21 = t;
}
//行列式求值
ssfloat ssFLOAT2x2X::Determinant()
{
	return m[0] * m[3] - m[1] * m[2];
}

bool ssFLOAT2x2X::Invert()
{
	float determinant = Determinant();
	if (ZERO_E5(determinant))
	{
		return false;
	}

	float tmp = m[0];   // copy the first element
	float invDeterminant = 1.0f / determinant;
	m[0] = invDeterminant * m[3];
	m[1] = -invDeterminant * m[1];
	m[2] = -invDeterminant * m[2];
	m[3] = invDeterminant * tmp;
	return true;
}

void ssFLOAT2x2X::operator *= (const ssFLOAT2x2X &val)
{
	ssFLOAT2x2X thism = *this;
	_11 = (thism._11*val._11) + (thism._12*val._21);
	_12 = (thism._11*val._12) + (thism._12*val._22);
	_21 = (thism._21*val._11) + (thism._22*val._21);
	_22 = (thism._21*val._12) + (thism._22*val._22);
}
void ssFLOAT2x2X::operator += (const ssFLOAT2x2X &val)
{
	_11 += val._11; _12 += val._12;
	_21 += val._21; _22 += val._22;
}
void ssFLOAT2x2X::operator -= (const ssFLOAT2x2X &val)
{
	_11 -= val._11; _12 -= val._12;
	_21 -= val._21; _22 -= val._22;
}
void ssFLOAT2x2X::operator *= (ssfloat val)
{
	_11 *= val; _12 *= val;
	_21 *= val; _22 *= val;

}
void ssFLOAT2x2X::operator /= (ssfloat val)
{
	SS_ASSERT(val != 0.0);
	ssfloat inv = 1.0f / val;
	_11 *= inv; _12 *= inv;
	_21 *= inv; _22 *= inv;
}

ssFLOAT2x2X ssFLOAT2x2X::operator * (const ssFLOAT2x2X &val) const
{
	ssFLOAT2x2X nm;
	nm._11 = (_11*val._11) + (_12*val._21);
	nm._12 = (_11*val._12) + (_12*val._22);
	nm._21 = (_21*val._11) + (_22*val._21);
	nm._22 = (_21*val._12) + (_22*val._22);
	return nm;
}
ssFLOAT2x2X ssFLOAT2x2X::operator + (const ssFLOAT2x2X &val) const
{
	return ssFLOAT2x2X(
		_11 + val._11, _12 + val._12,
		_21 + val._21, _22 + val._22);
}
ssFLOAT2x2X ssFLOAT2x2X::operator - (const ssFLOAT2x2X &val) const
{
	return ssFLOAT2x2X(
		_11 - val._11, _12 - val._12,
		_21 - val._21, _22 - val._22);
}
ssFLOAT2x2X ssFLOAT2x2X::operator * (ssfloat val) const
{
	return ssFLOAT2x2X(
		_11 * val, _12 * val,
		_21 * val, _22 * val);
}
ssFLOAT2x2X ssFLOAT2x2X::operator / (ssfloat val) const
{
	SS_ASSERT(val != 0.0);
	ssfloat inv = 1.0f / val;
	return ssFLOAT2x2X(
		_11 * inv, _12 * inv,
		_21 * inv, _22 * inv);
}
ssFLOAT2x2X ssFLOAT2x2X::operator-()
{
	return ssFLOAT2x2X(-m[0], -m[1], -m[2], -m[3]);
}

bool ssFLOAT2x2X::operator == (const ssFLOAT2x2X &val) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT2x2X)) == 0);
}
bool ssFLOAT2x2X::operator != (const ssFLOAT2x2X &val) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT2x2X)) != 0);
}
///////////////////////////////////////////////////////////////////////////
/////////////////////////////////float3x3//////////////////////////////////
ssFLOAT3x3X::ssFLOAT3x3X()
{
	Zero();
}

ssFLOAT3x3X::ssFLOAT3x3X(const ssfloat* val)
{
	memcpy(m, val, sizeof(ssFLOAT3x3));
}

ssFLOAT3x3X::~ssFLOAT3x3X()
{
}

ssFLOAT3x3X::ssFLOAT3x3X(ssfloat p11, ssfloat p12, ssfloat p13,
						ssfloat p21, ssfloat p22, ssfloat p23,
						ssfloat p31, ssfloat p32, ssfloat p33)
{
	_11=p11; _12=p12; _13=p13;
	_21=p21; _22=p22; _23=p23;
	_31=p31; _32=p32; _33=p33;
}

ssFLOAT3x3X::ssFLOAT3x3X( const ssFLOAT3x3X &val )
{
	memcpy(m, val.m, sizeof(ssFLOAT3x3));
}

void ssFLOAT3x3X::operator=(const ssFLOAT3x3X &val)
{
	memcpy(m, val.m, sizeof(ssFLOAT3x3));
}

ssFLOAT3x3X::operator ssfloat* ()
{
	return &_11;
}

ssFLOAT3x3X::operator const ssfloat* () const
{
	return &_11;
}

void ssFLOAT3x3X::Zero()
{
	memset(m, 0, sizeof(ssFLOAT3x3));
}
//单位化
void ssFLOAT3x3X::Identity()
{
	_12 = _13 =
	_21 = _23 =
	_31 = _32 = 0.0f;
	_11 = _22 = _33 = 1.0f;
}
//转置
void ssFLOAT3x3X::Transpose()
{
	ssfloat t;
	t = _12; _12 = _21; _21 = t;
	t = _13; _13 = _31; _31 = t;
	t = _23; _23 = _32; _32 = t;
}
//行列式求值
ssfloat ssFLOAT3x3X::Determinant()
{
	return m[0] * (m[4] * m[8] - m[5] * m[7]) -
			m[1] * (m[3] * m[8] - m[5] * m[6]) +
			m[2] * (m[3] * m[7] - m[4] * m[6]);
}

bool ssFLOAT3x3X::Invert()
{
	ssfloat determinant, invDeterminant;
	ssfloat tmp[9];

	tmp[0] = m[4] * m[8] - m[5] * m[7];
	tmp[1] = m[7] * m[2] - m[8] * m[1];
	tmp[2] = m[1] * m[5] - m[2] * m[4];
	tmp[3] = m[5] * m[6] - m[3] * m[8];
	tmp[4] = m[0] * m[8] - m[2] * m[6];
	tmp[5] = m[2] * m[3] - m[0] * m[5];
	tmp[6] = m[3] * m[7] - m[4] * m[6];
	tmp[7] = m[6] * m[1] - m[7] * m[0];
	tmp[8] = m[0] * m[4] - m[1] * m[3];

	// check determinant if it is 0
	determinant = m[0] * tmp[0] + m[1] * tmp[3] + m[2] * tmp[6];
	if (ZERO_E5(determinant))
	{
		return false;
	}

	// divide by the determinant
	invDeterminant = 1.0f / determinant;
	m[0] = invDeterminant * tmp[0];
	m[1] = invDeterminant * tmp[1];
	m[2] = invDeterminant * tmp[2];
	m[3] = invDeterminant * tmp[3];
	m[4] = invDeterminant * tmp[4];
	m[5] = invDeterminant * tmp[5];
	m[6] = invDeterminant * tmp[6];
	m[7] = invDeterminant * tmp[7];
	m[8] = invDeterminant * tmp[8];
	return true;
}

ssFLOAT3 ssFLOAT3x3X::GetAxisX()
{
	ssFLOAT3 v;
	v.x = m2[0][0];
	v.y = m2[0][1];
	v.z = m2[0][2];
	return v;
}

ssFLOAT3 ssFLOAT3x3X::GetAxisY()
{
	ssFLOAT3 v;
	v.x = m2[1][0];
	v.y = m2[1][1];
	v.z = m2[1][2];
	return v;
}

ssFLOAT3 ssFLOAT3x3X::GetAxisZ()
{
	ssFLOAT3 v;
	v.x = m2[2][0];
	v.y = m2[2][1];
	v.z = m2[2][2];
	return v;
}

void ssFLOAT3x3X::operator *= ( const ssFLOAT3x3X &val )
{
	ssFLOAT3x3X thism = *this;
	_11=(thism._11*val._11) + (thism._12*val._21) + (thism._13*val._31);
	_12=(thism._11*val._12) + (thism._12*val._22) + (thism._13*val._32);
	_13=(thism._11*val._13) + (thism._12*val._23) + (thism._13*val._33);
	_21=(thism._21*val._11) + (thism._22*val._21) + (thism._23*val._31);
	_22=(thism._21*val._12) + (thism._22*val._22) + (thism._23*val._32);
	_23=(thism._21*val._13) + (thism._22*val._23) + (thism._23*val._33);
	_31=(thism._31*val._11) + (thism._32*val._21) + (thism._33*val._31);
	_32=(thism._31*val._12) + (thism._32*val._22) + (thism._33*val._32);
	_33=(thism._31*val._13) + (thism._32*val._23) + (thism._33*val._33);
}
void ssFLOAT3x3X::operator += ( const ssFLOAT3x3X &val )
{
	_11+=val._11; _12+=val._12; _13+=val._13;
	_21+=val._21; _22+=val._22; _23+=val._23;
	_31+=val._31; _32+=val._32; _33+=val._33;
}
void ssFLOAT3x3X::operator -= ( const ssFLOAT3x3X &val )
{
	_11 -= val._11; _12 -= val._12; _13 -= val._13;
	_21 -= val._21; _22 -= val._22; _23 -= val._23;
	_31 -= val._31; _32 -= val._32; _33 -= val._33;
}
void ssFLOAT3x3X::operator *= ( ssfloat val )
{
	_11 *= val; _12 *= val; _13 *= val;
	_21 *= val; _22 *= val; _23 *= val;
	_31 *= val; _32 *= val; _33 *= val;

}
void ssFLOAT3x3X::operator /= ( ssfloat val )
{
	SS_ASSERT(val!=0.0);
	ssfloat inv = 1.0f / val;
	_11 *= inv; _12 *= inv; _13 *= inv;
	_21 *= inv; _22 *= inv; _23 *= inv;
	_31 *= inv; _32 *= inv; _33 *= inv;
}

ssFLOAT3x3X ssFLOAT3x3X::operator * ( const ssFLOAT3x3X &val ) const
{
	ssFLOAT3x3X nm;
	nm._11=(_11*val._11) + (_12*val._21) + (_13*val._31);
	nm._12=(_11*val._12) + (_12*val._22) + (_13*val._32);
	nm._13=(_11*val._13) + (_12*val._23) + (_13*val._33);
	nm._21=(_21*val._11) + (_22*val._21) + (_23*val._31);
	nm._22=(_21*val._12) + (_22*val._22) + (_23*val._32);
	nm._23=(_21*val._13) + (_22*val._23) + (_23*val._33);
	nm._31=(_31*val._11) + (_32*val._21) + (_33*val._31);
	nm._32=(_31*val._12) + (_32*val._22) + (_33*val._32);
	nm._33=(_31*val._13) + (_32*val._23) + (_33*val._33);
	return nm;
}
ssFLOAT3x3X ssFLOAT3x3X::operator + ( const ssFLOAT3x3X &val ) const
{
	return ssFLOAT3x3X(
		_11 + val._11, _12 + val._12, _13 + val._13,
		_21 + val._21, _22 + val._22, _23 + val._23,
		_31 + val._31, _32 + val._32, _33 + val._33);
}
ssFLOAT3x3X ssFLOAT3x3X::operator - ( const ssFLOAT3x3X &val ) const
{
	return ssFLOAT3x3X(
		_11 - val._11, _12 - val._12, _13 - val._13,
		_21 - val._21, _22 - val._22, _23 - val._23,
		_31 - val._31, _32 - val._32, _33 - val._33);
}
ssFLOAT3x3X ssFLOAT3x3X::operator * ( ssfloat val ) const
{
	return ssFLOAT3x3X(
		_11 * val, _12 * val, _13 * val,
		_21 * val, _22 * val, _23 * val,
		_31 * val, _32 * val, _33 * val);
}
ssFLOAT3x3X ssFLOAT3x3X::operator / ( ssfloat val ) const
{
	SS_ASSERT(val!=0.0);
	ssfloat inv = 1.0f / val;
	return ssFLOAT3x3X(
		_11 * inv, _12 * inv, _13 * inv,
		_21 * inv, _22 * inv, _23 * inv,
		_31 * inv, _32 * inv, _33 * inv);
}

bool ssFLOAT3x3X::operator == ( const ssFLOAT3x3X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT3x3X)) == 0);
}
bool ssFLOAT3x3X::operator != ( const ssFLOAT3x3X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT3x3X)) != 0);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////ssFLOAT4x4X////////////////////////////////
ssFLOAT4x4X::ssFLOAT4x4X()
{
	Zero();
}
ssFLOAT4x4X::ssFLOAT4x4X(const ssfloat* val)
{
	memcpy(m, val, sizeof(ssFLOAT4x4));
}
ssFLOAT4x4X::~ssFLOAT4x4X()
{
}
ssFLOAT4x4X::ssFLOAT4x4X(ssfloat p11, ssfloat p12, ssfloat p13, ssfloat p14,
						ssfloat p21, ssfloat p22, ssfloat p23, ssfloat p24,
						ssfloat p31, ssfloat p32, ssfloat p33, ssfloat p34,
						ssfloat p41, ssfloat p42, ssfloat p43, ssfloat p44)
{
	_11=p11; _12=p12; _13=p13; _14=p14;
	_21=p21; _22=p22; _23=p23; _24=p24;
	_31=p31; _32=p32; _33=p33; _34=p34;
	_41=p41; _42=p42; _43=p43; _44=p44;
}
ssFLOAT4x4X::ssFLOAT4x4X( const ssFLOAT4x4X &val )
{
	memcpy(m, val.m, sizeof(ssFLOAT4x4));
}
void ssFLOAT4x4X::operator=(const ssFLOAT4x4X &val)
{
	memcpy(m, val.m, sizeof(ssFLOAT4x4X));
}

ssFLOAT4x4X::operator ssfloat* ()
{
	return &_11;
}
ssFLOAT4x4X::operator const ssfloat* () const
{
	return &_11;
}

void ssFLOAT4x4X::Zero()
{
	memset(m, 0, sizeof(ssFLOAT4x4));
}
//单位化
void ssFLOAT4x4X::Identity()
{
	_12 = _13 = _14 =
	_21 = _23 = _24 =
	_31 = _32 = _34 =
	_41 = _42 = _43 = 0.0f;
	_11 = _22 = _33 = _44 = 1.0f;
}
//当作3x3矩阵
void ssFLOAT4x4X::Convert3x3()
{
	_14 = _24 = _34 = 0.0f;
	_41 = _42 = _43 = 0.0f;
	_44 = 1.0f;
}
//复制3x3矩阵的值
void ssFLOAT4x4X::CopyFrom3x3(ssFLOAT3x3X& f3)
{
	_11 = f3._11; _12 = f3._12; _13 = f3._13;
	_21 = f3._21; _22 = f3._22; _23 = f3._23;
	_31 = f3._31; _32 = f3._32; _33 = f3._33;
	_14 = _24 = _34 = 0.0f;
	_41 = _42 = _43 = 0.0f;
	_44 = 1.0f;
}
//转置
void ssFLOAT4x4X::Transpose()
{
	ssfloat t;
	t = _12; _12 = _21; _21 = t;
	t = _13; _13 = _31; _31 = t;
	t = _14; _14 = _41; _41 = t;
	t = _23; _23 = _32; _32 = t;
	t = _24; _24 = _42; _42 = t;
	t = _34; _34 = _43; _43 = t;
}
//行列式求值
ssfloat ssFLOAT4x4X::Determinant()
{
	return	_11*(_22*(_33*_44-_34*_43)+_23*(_34*_42-_32*_44)+_24*(_32*_43-_33*_42))-
			_12*(_21*(_33*_44-_34*_43)+_23*(_34*_41-_31*_44)+_24*(_31*_43-_33*_41))+
			_13*(_21*(_32*_44-_34*_42)+_22*(_34*_41-_31*_44)+_24*(_31*_42-_32*_41))-
			_14*(_21*(_32*_43-_33*_42)+_22*(_33*_41-_31*_43)+_23*(_31*_42-_32*_41));
}

ssfloat ssFLOAT4x4X::Cofactor(ssfloat m0, ssfloat m1, ssfloat m2,
							ssfloat m3, ssfloat m4, ssfloat m5,
							ssfloat m6, ssfloat m7, ssfloat m8)
{
	return m0 * (m4 * m8 - m5 * m7) -
		m1 * (m3 * m8 - m5 * m6) +
		m2 * (m3 * m7 - m4 * m6);
}

void ssFLOAT4x4X::InvertEuclidean()
{
	// transpose 3x3 rotation matrix part
	// | R^T | 0 |
	// | ----+-- |
	// |  0  | 1 |
	ssfloat tmp;
	tmp = m[1];  m[1] = m[4];  m[4] = tmp;
	tmp = m[2];  m[2] = m[8];  m[8] = tmp;
	tmp = m[6];  m[6] = m[9];  m[9] = tmp;

	// compute translation part -R^T * T
	// | 0 | -R^T x |
	// | --+------- |
	// | 0 |   0    |
	ssfloat x = m[12];
	ssfloat y = m[13];
	ssfloat z = m[14];
	m[12] = -(m[0] * x + m[4] * y + m[8] * z);
	m[13] = -(m[1] * x + m[5] * y + m[9] * z);
	m[14] = -(m[2] * x + m[6] * y + m[10] * z);
	// last row should be unchanged (0,0,0,1)
}

void ssFLOAT4x4X::InvertAffine()
{
	// R^-1
	ssFLOAT3x3X r(m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]);
	r.Invert();
	m[0] = r[0];  m[1] = r[1];  m[2] = r[2];
	m[4] = r[3];  m[5] = r[4];  m[6] = r[5];
	m[8] = r[6];  m[9] = r[7];  m[10] = r[8];

	// -R^-1 * T
	ssfloat x = m[12];
	ssfloat y = m[13];
	ssfloat z = m[14];
	m[12] = -(r[0] * x + r[3] * y + r[6] * z);
	m[13] = -(r[1] * x + r[4] * y + r[7] * z);
	m[14] = -(r[2] * x + r[5] * y + r[8] * z);

	// last row should be unchanged (0,0,0,1)
	//m[3] = m[7] = m[11] = 0.0f;
	//m[15] = 1.0f;
}

bool ssFLOAT4x4X::InvertProjective()
{
	// partition
	ssFLOAT2x2X a(m[0], m[1], m[4], m[5]);
	ssFLOAT2x2X b(m[8], m[9], m[12], m[13]);
	ssFLOAT2x2X c(m[2], m[3], m[6], m[7]);
	ssFLOAT2x2X d(m[10], m[11], m[14], m[15]);

	// pre-compute repeated parts
	a.Invert();					// A^-1
	ssFLOAT2x2X ab = a * b;     // A^-1 * B
	ssFLOAT2x2X ca = c * a;     // C * A^-1
	ssFLOAT2x2X cab = ca * b;   // C * A^-1 * B
	ssFLOAT2x2X dcab = d - cab; // D - C * A^-1 * B

	// check determinant if |D - C * A^-1 * B| = 0
	//NOTE: this function assumes det(A) is already checked. if |A|=0 then,
	// cannot use this function.
	float determinant = dcab[0] * dcab[3] - dcab[1] * dcab[2];
	if (ZERO_E5(determinant))
	{
		return false;
	}

	// compute D' and -D'
	ssFLOAT2x2X d1 = dcab;      //  (D - C * A^-1 * B)
	d1.Invert();				//  (D - C * A^-1 * B)^-1
	ssFLOAT2x2X d2 = -d1;       // -(D - C * A^-1 * B)^-1

	// compute C'
	ssFLOAT2x2X c1 = d2 * ca;   // -D' * (C * A^-1)

	// compute B'
	ssFLOAT2x2X b1 = ab * d2;   // (A^-1 * B) * -D'

	// compute A'
	ssFLOAT2x2X a1 = a - (ab * c1); // A^-1 - (A^-1 * B) * C'

	// assemble inverse matrix
	m[0] = a1[0];  m[4] = a1[2]; /*|*/ m[8] = b1[0];  m[12] = b1[2];
	m[1] = a1[1];  m[5] = a1[3]; /*|*/ m[9] = b1[1];  m[13] = b1[3];
	/*-----------------------------+-----------------------------*/
	m[2] = c1[0];  m[6] = c1[2]; /*|*/ m[10] = d1[0];  m[14] = d1[2];
	m[3] = c1[1];  m[7] = c1[3]; /*|*/ m[11] = d1[1];  m[15] = d1[3];

	return true;
}

bool ssFLOAT4x4X::InvertGeneral()
{
	// get cofactors of minor matrices
	float cofactor0 = Cofactor(m[5], m[6], m[7], m[9], m[10], m[11], m[13], m[14], m[15]);
	float cofactor1 = Cofactor(m[4], m[6], m[7], m[8], m[10], m[11], m[12], m[14], m[15]);
	float cofactor2 = Cofactor(m[4], m[5], m[7], m[8], m[9], m[11], m[12], m[13], m[15]);
	float cofactor3 = Cofactor(m[4], m[5], m[6], m[8], m[9], m[10], m[12], m[13], m[14]);

	// get determinant
	float determinant = m[0] * cofactor0 - m[1] * cofactor1 + m[2] * cofactor2 - m[3] * cofactor3;
	if (ZERO_E5(determinant))
	{
		return false;
	}

	// get rest of cofactors for adj(M)
	float cofactor4 = Cofactor(m[1], m[2], m[3], m[9], m[10], m[11], m[13], m[14], m[15]);
	float cofactor5 = Cofactor(m[0], m[2], m[3], m[8], m[10], m[11], m[12], m[14], m[15]);
	float cofactor6 = Cofactor(m[0], m[1], m[3], m[8], m[9], m[11], m[12], m[13], m[15]);
	float cofactor7 = Cofactor(m[0], m[1], m[2], m[8], m[9], m[10], m[12], m[13], m[14]);

	float cofactor8 = Cofactor(m[1], m[2], m[3], m[5], m[6], m[7], m[13], m[14], m[15]);
	float cofactor9 = Cofactor(m[0], m[2], m[3], m[4], m[6], m[7], m[12], m[14], m[15]);
	float cofactor10 = Cofactor(m[0], m[1], m[3], m[4], m[5], m[7], m[12], m[13], m[15]);
	float cofactor11 = Cofactor(m[0], m[1], m[2], m[4], m[5], m[6], m[12], m[13], m[14]);

	float cofactor12 = Cofactor(m[1], m[2], m[3], m[5], m[6], m[7], m[9], m[10], m[11]);
	float cofactor13 = Cofactor(m[0], m[2], m[3], m[4], m[6], m[7], m[8], m[10], m[11]);
	float cofactor14 = Cofactor(m[0], m[1], m[3], m[4], m[5], m[7], m[8], m[9], m[11]);
	float cofactor15 = Cofactor(m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]);

	// build inverse matrix = adj(M) / det(M)
	// adjugate of M is the transpose of the cofactor matrix of M
	float invDeterminant = 1.0f / determinant;
	m[0] = invDeterminant * cofactor0;
	m[1] = -invDeterminant * cofactor4;
	m[2] = invDeterminant * cofactor8;
	m[3] = -invDeterminant * cofactor12;

	m[4] = -invDeterminant * cofactor1;
	m[5] = invDeterminant * cofactor5;
	m[6] = -invDeterminant * cofactor9;
	m[7] = invDeterminant * cofactor13;

	m[8] = invDeterminant * cofactor2;
	m[9] = -invDeterminant * cofactor6;
	m[10] = invDeterminant * cofactor10;
	m[11] = -invDeterminant * cofactor14;

	m[12] = -invDeterminant * cofactor3;
	m[13] = invDeterminant * cofactor7;
	m[14] = -invDeterminant * cofactor11;
	m[15] = invDeterminant * cofactor15;

	return true;
}

bool ssFLOAT4x4X::Invert()
{
	// If the 4th row is [0,0,0,1] then it is affine matrix and
	// it has no projective transformation.
	if (m[3] == 0 && m[7] == 0 && m[11] == 0 && m[15] == 1)
	{
		this->InvertAffine();
		return true;
	}
	else
	{
		return this->InvertGeneral();
		/*@@ invertProjective() is not optimized (slower than generic one)
		if(fabs(m[0]*m[5] - m[1]*m[4]) > EPSILON)
		this->invertProjective();   // inverse using matrix partition
		else
		this->invertGeneral();      // generalized inverse
		*/
	}
	return false;
}

ssFLOAT3 ssFLOAT4x4X::GetAxisX()
{
	ssFLOAT3 v;
	v.x = m2[0][0];
	v.y = m2[0][1];
	v.z = m2[0][2];
	return v;
}

ssFLOAT3 ssFLOAT4x4X::GetAxisY()
{
	ssFLOAT3 v;
	v.x = m2[1][0];
	v.y = m2[1][1];
	v.z = m2[1][2];
	return v;
}

ssFLOAT3 ssFLOAT4x4X::GetAxisZ()
{
	ssFLOAT3 v;
	v.x = m2[2][0];
	v.y = m2[2][1];
	v.z = m2[2][2];
	return v;
}

ssFLOAT3 ssFLOAT4x4X::GetOriginPoint()
{
	ssFLOAT3 v;
	v.x = m2[3][0];
	v.y = m2[3][1];
	v.z = m2[3][2];
	return v;
}

void ssFLOAT4x4X::operator *= ( const ssFLOAT4x4X &val )
{
	ssFLOAT4x4X thism = *this;
	_11=(thism._11*val._11) + (thism._12*val._21) + (thism._13*val._31) + (thism._14*val._41);
	_12=(thism._11*val._12) + (thism._12*val._22) + (thism._13*val._32) + (thism._14*val._42);
	_13=(thism._11*val._13) + (thism._12*val._23) + (thism._13*val._33) + (thism._14*val._43);
	_14=(thism._11*val._14) + (thism._12*val._24) + (thism._13*val._34) + (thism._14*val._44);
	_21=(thism._21*val._11) + (thism._22*val._21) + (thism._23*val._31) + (thism._24*val._41);
	_22=(thism._21*val._12) + (thism._22*val._22) + (thism._23*val._32) + (thism._24*val._42);
	_23=(thism._21*val._13) + (thism._22*val._23) + (thism._23*val._33) + (thism._24*val._43);
	_24=(thism._21*val._14) + (thism._22*val._24) + (thism._23*val._34) + (thism._24*val._44);
	_31=(thism._31*val._11) + (thism._32*val._21) + (thism._33*val._31) + (thism._34*val._41);
	_32=(thism._31*val._12) + (thism._32*val._22) + (thism._33*val._32) + (thism._34*val._42);
	_33=(thism._31*val._13) + (thism._32*val._23) + (thism._33*val._33) + (thism._34*val._43);
	_34=(thism._31*val._14) + (thism._32*val._24) + (thism._33*val._34) + (thism._34*val._44);
	_41=(thism._41*val._11) + (thism._42*val._21) + (thism._43*val._31) + (thism._44*val._41);
	_42=(thism._41*val._12) + (thism._42*val._22) + (thism._43*val._32) + (thism._44*val._42);
	_43=(thism._41*val._13) + (thism._42*val._23) + (thism._43*val._33) + (thism._44*val._43);
	_44=(thism._41*val._14) + (thism._42*val._24) + (thism._43*val._34) + (thism._44*val._44);
}
void ssFLOAT4x4X::operator += ( const ssFLOAT4x4X &val )
{
	_11+=val._11; _12+=val._12; _13+=val._13; _14+=val._14;
	_21+=val._21; _22+=val._22; _23+=val._23; _24+=val._24;
	_31+=val._31; _32+=val._32; _33+=val._33; _34+=val._34;
	_41+=val._41; _42+=val._42; _43+=val._43; _44+=val._44;
}
void ssFLOAT4x4X::operator -= ( const ssFLOAT4x4X &val )
{
	_11 -= val._11; _12 -= val._12; _13 -= val._13; _14 -= val._14;
	_21 -= val._21; _22 -= val._22; _23 -= val._23; _24 -= val._24;
	_31 -= val._31; _32 -= val._32; _33 -= val._33; _34 -= val._34;
	_41 -= val._41; _42 -= val._42; _43 -= val._43; _44 -= val._44;
}
void ssFLOAT4x4X::operator *= ( ssfloat val )
{
	_11 *= val; _12 *= val; _13 *= val; _14 *= val;
	_21 *= val; _22 *= val; _23 *= val; _24 *= val;
	_31 *= val; _32 *= val; _33 *= val; _34 *= val;
	_41 *= val; _42 *= val; _43 *= val; _44 *= val;

}
void ssFLOAT4x4X::operator /= ( ssfloat val )
{
	SS_ASSERT(val!=0.0);
	ssfloat inv = 1.0f / val;
	_11 *= inv; _12 *= inv; _13 *= inv; _14 *= inv;
	_21 *= inv; _22 *= inv; _23 *= inv; _24 *= inv;
	_31 *= inv; _32 *= inv; _33 *= inv; _34 *= inv;
	_41 *= inv; _42 *= inv; _43 *= inv; _44 *= inv;
}

ssFLOAT4x4X ssFLOAT4x4X::operator * ( const ssFLOAT4x4X &val ) const
{
	ssFLOAT4x4X nm;
	nm._11=(_11*val._11) + (_12*val._21) + (_13*val._31) + (_14*val._41);
	nm._12=(_11*val._12) + (_12*val._22) + (_13*val._32) + (_14*val._42);
	nm._13=(_11*val._13) + (_12*val._23) + (_13*val._33) + (_14*val._43);
	nm._14=(_11*val._14) + (_12*val._24) + (_13*val._34) + (_14*val._44);
	nm._21=(_21*val._11) + (_22*val._21) + (_23*val._31) + (_24*val._41);
	nm._22=(_21*val._12) + (_22*val._22) + (_23*val._32) + (_24*val._42);
	nm._23=(_21*val._13) + (_22*val._23) + (_23*val._33) + (_24*val._43);
	nm._24=(_21*val._14) + (_22*val._24) + (_23*val._34) + (_24*val._44);
	nm._31=(_31*val._11) + (_32*val._21) + (_33*val._31) + (_34*val._41);
	nm._32=(_31*val._12) + (_32*val._22) + (_33*val._32) + (_34*val._42);
	nm._33=(_31*val._13) + (_32*val._23) + (_33*val._33) + (_34*val._43);
	nm._34=(_31*val._14) + (_32*val._24) + (_33*val._34) + (_34*val._44);
	nm._41=(_41*val._11) + (_42*val._21) + (_43*val._31) + (_44*val._41);
	nm._42=(_41*val._12) + (_42*val._22) + (_43*val._32) + (_44*val._42);
	nm._43=(_41*val._13) + (_42*val._23) + (_43*val._33) + (_44*val._43);
	nm._44=(_41*val._14) + (_42*val._24) + (_43*val._34) + (_44*val._44);
	return nm;
}
ssFLOAT4x4X ssFLOAT4x4X::operator + ( const ssFLOAT4x4X &val ) const
{
	return ssFLOAT4x4X(
		_11 + val._11, _12 + val._12, _13 + val._13, _14 + val._14,
		_21 + val._21, _22 + val._22, _23 + val._23, _24 + val._24,
		_31 + val._31, _32 + val._32, _33 + val._33, _34 + val._34,
		_41 + val._41, _42 + val._42, _43 + val._43, _44 + val._44);
}
ssFLOAT4x4X ssFLOAT4x4X::operator - ( const ssFLOAT4x4X &val ) const
{
	return ssFLOAT4x4X(
		_11 - val._11, _12 - val._12, _13 - val._13, _14 - val._14,
		_21 - val._21, _22 - val._22, _23 - val._23, _24 - val._24,
		_31 - val._31, _32 - val._32, _33 - val._33, _34 - val._34,
		_41 - val._41, _42 - val._42, _43 - val._43, _44 - val._44);
}
ssFLOAT4x4X ssFLOAT4x4X::operator * ( ssfloat val ) const
{
	return ssFLOAT4x4X(
		_11 * val, _12 * val, _13 * val, _14 * val,
		_21 * val, _22 * val, _23 * val, _24 * val,
		_31 * val, _32 * val, _33 * val, _34 * val,
		_41 * val, _42 * val, _43 * val, _44 * val);
}
ssFLOAT4x4X ssFLOAT4x4X::operator / ( ssfloat val ) const
{
	SS_ASSERT(val!=0.0);
	ssfloat inv = 1.0f / val;
	return ssFLOAT4x4X(
		_11 * inv, _12 * inv, _13 * inv, _14 * inv,
		_21 * inv, _22 * inv, _23 * inv, _24 * inv,
		_31 * inv, _32 * inv, _33 * inv, _34 * inv,
		_41 * inv, _42 * inv, _43 * inv, _44 * inv);
}

bool ssFLOAT4x4X::operator == ( const ssFLOAT4x4X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT4x4X)) == 0);
}
bool ssFLOAT4x4X::operator != ( const ssFLOAT4x4X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT4x4X)) != 0);
}
///////////////////////////////////////////////////////////////////////////
/////////////////////////////////float4x3//////////////////////////////////
ssFLOAT4x3X::ssFLOAT4x3X()
{
	Zero();
}
ssFLOAT4x3X::~ssFLOAT4x3X()
{
}
ssFLOAT4x3X::ssFLOAT4x3X(ssfloat p11, ssfloat p12, ssfloat p13,
						ssfloat p21, ssfloat p22, ssfloat p23,
						ssfloat p31, ssfloat p32, ssfloat p33,
						ssfloat p41, ssfloat p42, ssfloat p43)
{
	_11=p11; _12=p12; _13=p13;
	_21=p21; _22=p22; _23=p23;
	_31=p31; _32=p32; _33=p33;
	_41=p41; _42=p42; _43=p43;
}
ssFLOAT4x3X::ssFLOAT4x3X( const ssFLOAT4x3X &val )
{
	memcpy(m, val.m, sizeof(ssFLOAT4x3));
}

void ssFLOAT4x3X::operator=(const ssFLOAT4x3 &val)
{
	memcpy(m, val.m, sizeof(ssFLOAT4x3));
}

ssFLOAT4x3X::operator ssfloat* ()
{
	return &_11;
}
ssFLOAT4x3X::operator const ssfloat* () const
{
	return &_11;
}

void ssFLOAT4x3X::Zero()
{
	memset(m, 0, sizeof(ssFLOAT4x3));
}
//单位化
void ssFLOAT4x3X::Identity()
{
	_12=_13=_21=_23=
	_31=_32=_41=_42=_43=0.f;
	_11=_22=_33=1.f;
}
//行列式求值当做4x4矩阵
ssfloat ssFLOAT4x3X::Determinant4x4()
{
	//ssfloat ad[3] = {_11-_41, _12-_42, _13-_43};
	//ssfloat bd[3] = {_21-_41, _22-_42, _23-_43};
	//ssfloat cd[3] = {_31-_41, _32-_42, _33-_43};
	//ssfloat de[3] = {bd[1]*cd[2]-bd[2]*cd[1], bd[2]*cd[0]-bd[0]*cd[2], bd[0]*cd[1]-bd[1]*cd[0]};
	//return ad[0]*de[0]+ad[1]*de[1]+ad[2]*de[2];
	return (_11-_41)*((_22-_42)*(_33-_43)-(_23-_43)*(_32-_42))+
		(_12-_42)*((_23-_43)*(_31-_41)-(_21-_41)*(_33-_43))+
		( _13-_43)*((_21-_41)*(_32-_42)-(_22-_42)*(_31-_41));
}

bool ssFLOAT4x3X::operator == ( const ssFLOAT4x3X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT4x3X)) == 0);
}
bool ssFLOAT4x3X::operator != ( const ssFLOAT4x3X &val ) const
{
	return (memcmp(this, &val, sizeof(ssFLOAT4x3X)) != 0);
}
///////////////////////////////////////////////////////////////////////////
}
